
#include <AFMotor.h>
#include <SoftwareSerial.h>

// Set up a new SoftwareSerial object with RX in digital pin 10 and TX in digital pin 11
SoftwareSerial mySerial(10, 9);
AF_DCMotor motor1(1);
AF_DCMotor motor2(2);

AF_DCMotor motor[2]={motor1,motor2};
#define MOTORNUM  2
#define RIGHT A0
#define LEFT  A1
//speed 小于120时，启动不了
void forward_drive(int speed)
{
 
 for(char i=0;i<MOTORNUM;i++)
  {
     motor[i].run(FORWARD);
     motor[i].setSpeed(speed); 
   // motor[i].run(RELEASE);

  }
}
void left_drive(int speed1,int speed2)
{
    motor[0].run(FORWARD);
    motor[0].setSpeed(speed1); 
  // motor[i].run(RELEASE);
    motor[1].run(FORWARD); 
    motor[1].setSpeed(speed2); 
  


}
void spin(int left,int right)
{
    motor[0].run(FORWARD);
    motor[0].setSpeed(left); 
  // motor[i].run(RELEASE);
    motor[1].run(FORWARD); 
    motor[1].setSpeed(right); 
  

}
void right_drive(int speed1,int speed2)
{
    motor[0].run(FORWARD);
    motor[0].setSpeed(speed2); 
  // motor[i].run(RELEASE);
    motor[1].run(FORWARD); 
    motor[1].setSpeed(speed1); 
 


}
void backward_drive(int speed)
{
for(char i=0;i<MOTORNUM;i++)
  {
     motor[i].run(BACKWARD);
      motor[i].setSpeed(speed); 
   // motor[i].run(RELEASE);

  }

}
void rotate(int rot)
{
   char dir=0;
   char dir_1=0;
  if(rot>0)
  {
    dir=BACKWARD;
    dir_1=FORWARD;


  }
  else
  {
    dir=FORWARD;
    dir_1=BACKWARD;

  }
 
  for(char i=0;i<MOTORNUM;i++)
  {
      if(i==0||i==2)
      {
        motor[i].run(dir);
        motor[i].setSpeed(rot); 
      }
      if(i==1||i==3)
      {
        motor[i].run(dir_1);
        motor[i].setSpeed(rot); 
      }
   // motor[i].run(RELEASE);
  }

  
}
void stop()
{
  for(char i=0;i<MOTORNUM;i++)
  {
    motor[i].setSpeed(0); 
    motor[i].run(RELEASE);

  }

}
void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);           // set up Serial library at 9600 bps
  Serial.println("Motor test!");
  mySerial.begin(9600);
  // turn on motor
 for(char i=0;i<MOTORNUM;i++)
  {
    motor[i].setSpeed(200); 
    motor[i].run(RELEASE);

  }

  pinMode(RIGHT, INPUT); //定义右循迹红外传感器为输入
  pinMode(LEFT, INPUT); //定义左循迹红外传感器为输入

 
 
}

void loop() {
  // put your main code here, to run repeatedly:
//前进
  int SL;    //左循迹红外传感器状态
  int SR;    //右循迹红外传感器状态
  while(1)
  {
    SR = analogRead(RIGHT);//有信号表明在白色区域，车子底板上L1亮；没信号表明压在黑线上，车子底板上L1灭
    SL = analogRead(LEFT);//有信号表明在白色区域，车子底板上L2亮；没信号表明压在黑线上，车子底板上L2灭
   // delay(1000);
    
    if(SR>1000&&SL<100)
    {
     //  stop();

      spin(130,10);
    }
    if(SR>1000&&SL>1000)
    {
      stop();

    }
    if(SR<100&&SL<100)
    {
     //  stop();

      spin(50,50); 
    }
    if(SR<100&&SL>1000)
    {
     //  stop();

      spin(10,130);
    }
  }
  
  //step2. back 1m

}
